/*
 * pid.c
 *
 *  Created on: Sep 13, 2019
 *      Author: ROG
 */


#include "pid.h"

extern short _speed;
short roll = 0;
static float p = 1.2,i = 0.03 ,d = 0.03,error = 0,error_i = 0,error_d = 0;

short PID_MOTOR(short target)
{
	short speed;
	uint8_t id;


	MOTOR_GET_SPEED(&speed, &id);

	roll = error;

	error_d = (target - speed) - error;
	error = target - speed;
	error_i += error;
	MOTOR_SPEED(error * p + error_i * i + error_d * d);
	return speed;
}
